using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuUtilities
{
    public struct QuaternionWide
    {
        public Vector<float> X;
        public Vector<float> Y;
        public Vector<float> Z;
        public Vector<float> W;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Broadcast(in Quaternion source, out QuaternionWide broadcasted)
        {
            broadcasted.X = new Vector<float>(source.X);
            broadcasted.Y = new Vector<float>(source.Y);
            broadcasted.Z = new Vector<float>(source.Z);
            broadcasted.W = new Vector<float>(source.W);
        }

        /// <summary>
        /// 从源四元数中获取一个插槽,并将其广播到目标四元数的所有插槽中。
        /// </summary>
        /// <param name="source">要从中拉取值的四元数。</param>
        /// <param name="slotIndex">要从中拉取值的源向量中的槽。</param>
        /// <param name="Broadcast">要填充所选数据的目标四元数。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Rebroadcast(in QuaternionWide source, int slotIndex, out QuaternionWide broadcasted)
        { 
            broadcasted.X = new Vector<float>(source.X[slotIndex]);
            broadcasted.Y = new Vector<float>(source.Y[slotIndex]);
            broadcasted.Z = new Vector<float>(source.Z[slotIndex]);
            broadcasted.W = new Vector<float>(source.W[slotIndex]);
        }

        /// <summary>
        /// 从旋转矩阵构造一个四元数。
        /// </summary>
        /// <param name="r">要从中创建四元数的旋转矩阵。</param>
        /// <param name="q">基于旋转矩阵的四元数。</param>
        public static void CreateFromRotationMatrix(in Matrix3x3Wide r, out QuaternionWide q)
        {
            // 因为我们不能分支,所以我们最终要计算所有分支的可能状态。
            // 这需要做比分支实现更多的ALU工作,但是分支之间有很多公共术语,(随机的)分支不是免费的。
            // 总体而言,与标量版本相比,每个调用的成本大约高出2倍-2.5倍,但它可以处理多个通道,因此它是一个净赢家。
            var oneAddX = Vector<float>.One + r.X.X;
            var oneSubX = Vector<float>.One - r.X.X;
            var yAddZ = r.Y.Y + r.Z.Z;
            var ySubZ = r.Y.Y - r.Z.Z;
            var tX = oneAddX - yAddZ;
            var tY = oneSubX + ySubZ;
            var tZ = oneSubX - ySubZ;
            var tW = oneAddX + yAddZ;

            // 有两层条件--内部条件和外部条件。我们必须首先选择两个内半部分-上半部分和下半部分-
            // 然后我们将选择两个内部中的哪一个用于外部。
            var useUpper = Vector.LessThan(r.Z.Z, Vector<float>.Zero);
            var useUpperUpper = Vector.GreaterThan(r.X.X, r.Y.Y);
            var useLowerUpper = Vector.LessThan(r.X.X, -r.Y.Y);
            var t = Vector.ConditionalSelect(useUpper,
                    Vector.ConditionalSelect(useUpperUpper, tX, tY),
                    Vector.ConditionalSelect(useLowerUpper, tZ, tW));
            var xyAddYx = r.X.Y + r.Y.X;
            var yzSubZy = r.Y.Z - r.Z.Y;
            var zxAddXz = r.Z.X + r.X.Z;
            q.X = Vector.ConditionalSelect(useUpper,
                    Vector.ConditionalSelect(useUpperUpper, tX, xyAddYx),
                    Vector.ConditionalSelect(useLowerUpper, zxAddXz, yzSubZy));
            var yzAddZy = r.Y.Z + r.Z.Y;
            var zxSubXz = r.Z.X - r.X.Z;
            q.Y = Vector.ConditionalSelect(useUpper,
                    Vector.ConditionalSelect(useUpperUpper, xyAddYx, tY),
                    Vector.ConditionalSelect(useLowerUpper, yzAddZy, zxSubXz));
            var xySubYx = r.X.Y - r.Y.X;
            q.Z = Vector.ConditionalSelect(useUpper,
                    Vector.ConditionalSelect(useUpperUpper, zxAddXz, yzAddZy),
                    Vector.ConditionalSelect(useLowerUpper, tZ, xySubYx));
            q.W = Vector.ConditionalSelect(useUpper,
                    Vector.ConditionalSelect(useUpperUpper, yzSubZy, zxSubXz),
                    Vector.ConditionalSelect(useLowerUpper, xySubYx, tW));

            var scale = new Vector<float>(0.5f) / Vector.SquareRoot(t);
            Scale(q, scale, out q);
        }

        /// <summary>
        /// 将两个四元数的组件相加。
        /// </summary>
        /// <param name="a">要添加的第一个四元数。</param>
        /// <param name="b">要添加的第二个四元数。</param>
        /// <param name="result">两个输入四元数之和。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Add(in QuaternionWide a, in QuaternionWide b, out QuaternionWide result)
        {
            result.X = a.X + b.X;
            result.Y = a.Y + b.Y;
            result.Z = a.Z + b.Z;
            result.W = a.W + b.W;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Scale(in QuaternionWide q, in Vector<float> scale, out QuaternionWide result)
        {
            result.X = q.X * scale;
            result.Y = q.Y * scale;
            result.Z = q.Z * scale;
            result.W = q.W * scale;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void GetLengthSquared(in QuaternionWide q, out Vector<float> lengthSquared)
        {
            lengthSquared = q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W;
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void GetLength(in QuaternionWide q, out Vector<float> length)
        {
            length = Vector.SquareRoot(q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W);
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Normalize(in QuaternionWide q, out QuaternionWide normalized)
        {
            var inverseNorm = Vector<float>.One / Vector.SquareRoot(q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W);
            normalized.X = q.X * inverseNorm;
            normalized.Y = q.Y * inverseNorm;
            normalized.Z = q.Z * inverseNorm;
            normalized.W = q.W * inverseNorm;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Negate(in QuaternionWide q, out QuaternionWide negated)
        {
            negated.X = -q.X;
            negated.Y = -q.Y;
            negated.Z = -q.Z;
            negated.W = -q.W;
        }

        /// <summary>
        /// 计算两个规格化向量之间的四元数旋转。
        /// </summary>
        /// <param name="v1">第一个单位长度矢量。</param>
        /// <param name="v2">第二个单位长度矢量。</param>
        /// <param name="q">表示从v1到v2的旋转的四元数。</param>
        public static void GetQuaternionBetweenNormalizedVectors(in Vector3Wide v1, in Vector3Wide v2, out QuaternionWide q)
        {
            Vector3Wide.Dot(v1, v2, out var dot);
            // 对于非法线向量,需要将轴长平方相乘：
            // Float w=dot+sqrt(v1.LengthSquared()*v2.LengthSquared());


            // 点==-1处存在歧义。如果方向彼此背离,则有无限多条最短路径。
            // 其中一个必须是任意选择的。在这里,我们通过投影到法线与最小星等相关的平面来选择一个。
            // 由于这是SIMD操作,因此始终执行特殊情况,并有条件地选择其结果。

            Vector3Wide.CrossWithoutOverlap(v1, v2, out var cross);
            var useNormalCase = Vector.GreaterThan(dot, new Vector<float>(-0.999999f));
            var absX = Vector.Abs(v1.X);
            var absY = Vector.Abs(v1.Y);
            var absZ = Vector.Abs(v1.Z);
            var xIsSmallest = Vector.BitwiseAnd(Vector.LessThan(absX, absY), Vector.LessThan(absX, absZ));
            var yIsSmaller = Vector.LessThan(absY, absZ);
            q.X = Vector.ConditionalSelect(useNormalCase, cross.X, Vector.ConditionalSelect(xIsSmallest, Vector<float>.Zero, Vector.ConditionalSelect(yIsSmaller, -v1.Z, -v1.Y)));
            q.Y = Vector.ConditionalSelect(useNormalCase, cross.Y, Vector.ConditionalSelect(xIsSmallest, -v1.Z, Vector.ConditionalSelect(yIsSmaller, Vector<float>.Zero, v1.X)));
            q.Z = Vector.ConditionalSelect(useNormalCase, cross.Z, Vector.ConditionalSelect(xIsSmallest, v1.Y, Vector.ConditionalSelect(yIsSmaller, v1.X, Vector<float>.Zero)));
            q.W = Vector.ConditionalSelect(useNormalCase, dot + Vector<float>.One, Vector<float>.Zero);

            Normalize(q, out q);
        }

        /// <summary>
        /// 获取存储在四元数中的旋转的轴和角度表示形式。角度是近似的。
        /// </summary>
        /// <param name="q">从中提取轴角表示的四元数。</param>
        /// <param name="axis">从四元数中提取的旋转轴。</param>
        /// <param name="angle">从四元数提取的近似旋转角度。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void GetApproximateAxisAngleFromQuaternion(in QuaternionWide q, out Vector3Wide axis, out Vector<float> angle)
        {
            var shouldNegate = Vector.LessThan(q.W, Vector<float>.Zero);
            axis.X = Vector.ConditionalSelect(shouldNegate, -q.X, q.X);
            axis.Y = Vector.ConditionalSelect(shouldNegate, -q.Y, q.Y);
            axis.Z = Vector.ConditionalSelect(shouldNegate, -q.Z, q.Z);
            var qw = Vector.ConditionalSelect(shouldNegate, -q.W, q.W);

            Vector3Wide.Length(axis, out var axisLength);
            Vector3Wide.Scale(axis, Vector<float>.One / axisLength, out axis);
            var useFallback = Vector.LessThan(axisLength, new Vector<float>(1e-14f));
            axis.X = Vector.ConditionalSelect(useFallback, Vector<float>.One, axis.X);
            axis.Y = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, axis.Y);
            axis.Z = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, axis.Z);
            MathHelper.ApproximateAcos(qw, out var halfAngle);
            angle = 2 * halfAngle;
        }

        /// <summary>
        /// 使用四元数变换向量。假定支持输入和输出的内存不重叠。
        /// </summary>
        /// <param name="v">要转换的向量。</param>
        /// <param name="Rotation">应用于向量的旋转。</param>
        /// <param name="result">转换后的向量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void TransformWithoutOverlap(in Vector3Wide v, in QuaternionWide rotation, out Vector3Wide result)
        {
            // 此运算是v‘=q*v*q^-1的优化向下版本。
            // 展开后的形式将是将v视为"仅轴"四元数。
            // 并执行标准的四元数乘法。假设Q是归一化的,
            // q^-1可以用共轭来代替。
            var x2 = rotation.X + rotation.X;
            var y2 = rotation.Y + rotation.Y;
            var z2 = rotation.Z + rotation.Z;
            var xx2 = rotation.X * x2;
            var xy2 = rotation.X * y2;
            var xz2 = rotation.X * z2;
            var yy2 = rotation.Y * y2;
            var yz2 = rotation.Y * z2;
            var zz2 = rotation.Z * z2;
            var wx2 = rotation.W * x2;
            var wy2 = rotation.W * y2;
            var wz2 = rotation.W * z2;
            result.X = v.X * (Vector<float>.One - yy2 - zz2) + v.Y * (xy2 - wz2) + v.Z * (xz2 + wy2);
            result.Y = v.X * (xy2 + wz2) + v.Y * (Vector<float>.One - xx2 - zz2) + v.Z * (yz2 - wx2);
            result.Z = v.X * (xz2 - wy2) + v.Y * (yz2 + wx2) + v.Z * (Vector<float>.One - xx2 - yy2);

        }

        /// <summary>
        /// 使用四元数变换向量。
        /// </summary>
        /// <param name="v">要转换的向量。</param>
        /// <param name="Rotation">应用于向量的旋转。</param>
        /// <param name="result">转换后的向量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Transform(in Vector3Wide v, in QuaternionWide rotation, out Vector3Wide result)
        {
            TransformWithoutOverlap(v, rotation, out var temp);
            result = temp;
        }

        /// <summary>
        /// 使用四元数变换单位X方向。
        /// </summary>
        /// <param name="Rotation">应用于向量的旋转。</param>
        /// <param name="result">转换后的向量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void TransformUnitX(in QuaternionWide rotation, out Vector3Wide result)
        {
            var y2 = rotation.Y + rotation.Y;
            var z2 = rotation.Z + rotation.Z;
            var xy2 = rotation.X * y2;
            var xz2 = rotation.X * z2;
            var yy2 = rotation.Y * y2;
            var zz2 = rotation.Z * z2;
            var wy2 = rotation.W * y2;
            var wz2 = rotation.W * z2;
            result.X = Vector<float>.One - yy2 - zz2;
            result.Y = xy2 + wz2;
            result.Z = xz2 - wy2;
        }

        /// <summary>
        /// 使用四元数变换单位Y向量。
        /// </summary>
        /// <param name="Rotation">应用于向量的旋转。</param>
        /// <param name="result">转换后的向量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void TransformUnitY(in QuaternionWide rotation, out Vector3Wide result)
        {
            var x2 = rotation.X + rotation.X;
            var y2 = rotation.Y + rotation.Y;
            var z2 = rotation.Z + rotation.Z;
            var xx2 = rotation.X * x2;
            var xy2 = rotation.X * y2;
            var yz2 = rotation.Y * z2;
            var zz2 = rotation.Z * z2;
            var wx2 = rotation.W * x2;
            var wz2 = rotation.W * z2;
            result.X = xy2 - wz2;
            result.Y = Vector<float>.One - xx2 - zz2;
            result.Z = yz2 + wx2;
        }

        /// <summary>
        /// 使用四元数变换单位Z向量。
        /// </summary>
        /// <param name="Rotation">应用于向量的旋转。</param>
        /// <param name="result">转换后的向量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void TransformUnitZ(in QuaternionWide rotation, out Vector3Wide result)
        {
            var x2 = rotation.X + rotation.X;
            var y2 = rotation.Y + rotation.Y;
            var z2 = rotation.Z + rotation.Z;
            var xx2 = rotation.X * x2;
            var xz2 = rotation.X * z2;
            var yy2 = rotation.Y * y2;
            var yz2 = rotation.Y * z2;
            var wx2 = rotation.W * x2;
            var wy2 = rotation.W * y2;
            result.X = xz2 + wy2;
            result.Y = yz2 - wx2;
            result.Z = Vector<float>.One - xx2 - yy2;
        }

        /// <summary>
        /// 使用四元数变换单位X和单位Y方向。
        /// </summary>
        /// <param name="Rotation">应用于矢量的旋转。</param>
        /// <param name="x">转换的单位X向量。</param>
        /// <param name="y">转换的单位Y矢量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void TransformUnitXY(in QuaternionWide rotation, out Vector3Wide x, out Vector3Wide y)
        {
            var x2 = rotation.X + rotation.X;
            var y2 = rotation.Y + rotation.Y;
            var z2 = rotation.Z + rotation.Z;
            var xx2 = rotation.X * x2;
            var xy2 = rotation.X * y2;
            var xz2 = rotation.X * z2;
            var yy2 = rotation.Y * y2;
            var yz2 = rotation.Y * z2;
            var zz2 = rotation.Z * z2;
            var wx2 = rotation.W * x2;
            var wy2 = rotation.W * y2;
            var wz2 = rotation.W * z2;
            x.X = Vector<float>.One - yy2 - zz2;
            x.Y = xy2 + wz2;
            x.Z = xz2 - wy2;
            y.X = xy2 - wz2;
            y.Y = Vector<float>.One - xx2 - zz2;
            y.Z = yz2 + wx2;
        }

        /// <summary>
        /// 使用四元数变换单位X和单位Z方向。
        /// </summary>
        /// <param name="Rotation">应用于矢量的旋转。</param>
        /// <param name="x">转换的单位X向量。</param>
        /// <param name="z">转换的单位Z向量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void TransformUnitXZ(in QuaternionWide rotation, out Vector3Wide x, out Vector3Wide z)
        {
            var qX2 = rotation.X + rotation.X;
            var qY2 = rotation.Y + rotation.Y;
            var qZ2 = rotation.Z + rotation.Z;

            var YY = qY2 * rotation.Y;
            var ZZ = qZ2 * rotation.Z;
            x.X = Vector<float>.One - YY - ZZ;
            var XY = qX2 * rotation.Y;
            var ZW = qZ2 * rotation.W;
            x.Y = XY + ZW;
            var XZ = qX2 * rotation.Z;
            var YW = qY2 * rotation.W;
            x.Z = XZ - YW;

            var XX = qX2 * rotation.X;
            var XW = qX2 * rotation.W;
            var YZ = qY2 * rotation.Z;
            z.X = XZ + YW;
            z.Y = YZ - XW;
            z.Z = Vector<float>.One - XX - YY;
        }


        /// <summary>
        /// 将两个四元数的变换串联在一起,以使作为向量v的方向应用的结果四元数等效于
        /// 已转换=(v*a)*b。假设支持输入和输出的内存不重叠。
        /// </summary>
        /// <param name="a">要串联的第一个四元数。</param>
        /// <param name="b">要串联的第二个四元数。</param>
        /// <param name="result">串联的乘积。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ConcatenateWithoutOverlap(in QuaternionWide a, in QuaternionWide b, out QuaternionWide result)
        {
            result.X = a.W * b.X + a.X * b.W + a.Z * b.Y - a.Y * b.Z;
            result.Y = a.W * b.Y + a.Y * b.W + a.X * b.Z - a.Z * b.X;
            result.Z = a.W * b.Z + a.Z * b.W + a.Y * b.X - a.X * b.Y;
            result.W = a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z;
        }

        /// <summary>
        /// 将两个四元数的变换串联在一起,以使作为向量v的方向应用的结果四元数等效于
        /// 变换=(v*a)*b。
        /// </summary>
        /// <param name="a">要串联的第一个四元数。</param>
        /// <param name="b">要串联的第二个四元数。</param>
        /// <param name="result">串联的乘积。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Concatenate(in QuaternionWide a, in QuaternionWide b, out QuaternionWide result)
        {
            ConcatenateWithoutOverlap(a, b, out var tempResult);
            result = tempResult;
        }

        /// <summary>
        /// 计算四元数的共轭。
        /// </summary>
        /// <param name="四元数">要共轭的四元数。</param>
        /// <param name="result">共轭四元数。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Conjugate(in QuaternionWide quaternion, out QuaternionWide result)
        {
            result.X = quaternion.X;
            result.Y = quaternion.Y;
            result.Z = quaternion.Z;
            result.W = -quaternion.W;
        }

        /// <summary>
        /// 从宽四元数的第一个插槽收集值,并将它们放入一个狭窄的表示形式中。
        /// </summary>
        /// <param name="source">要从中复制值的宽四元数。</param>
        /// <param name="target">要将值放入的窄四元数。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ReadFirst(in QuaternionWide source, out Quaternion target)
        {
            // TODO：签入索引器上的参数codegen。
            target.X = source.X[0];
            target.Y = source.Y[0];
            target.Z = source.Z[0];
            target.W = source.W[0];
        }

        /// <summary>
        /// 从四元数收集值,并将它们放置到目标宽四元数的第一个索引中。
        /// </summary>
        /// <param name="source">要从中复制值的四元数。</param>
        /// <param name="targetSlot">要将值放入的宽四元数。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void WriteFirst(in Quaternion source, ref QuaternionWide targetSlot)
        {
            GatherScatter.GetFirst(ref targetSlot.X) = source.X;
            GatherScatter.GetFirst(ref targetSlot.Y) = source.Y;
            GatherScatter.GetFirst(ref targetSlot.Z) = source.Z;
            GatherScatter.GetFirst(ref targetSlot.W) = source.W;
        }
    }
}
